Artificial Intelligence

This section is allocated to the AI and robot modeling algorithms that will be discussed further later!

Robots Kinematic models

In this section I will describe the kinematic models of the robots and provide a code for estimating the end-effector position and etc.

UR5 Robot

#Math and numpy library import
from math import *
import numpy as np

#Translation matrixes for x and z
def TXZ(a,b):
        if b == 'x':
                return [[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
        elif b == 'z':
                return [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, a], [0, 0, 0, 1]]

#Rotation matrix for x and z
def RXZ(a,b):
        c, s = cos(a), sin(a)
        if b == 'x':
                return [[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]
        if b== 'z':
                return [[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]

#Euler angles calcluations to obtain Roll Pitch Yaw in degree
def HM2RPY(HM):
        RX = atan2(HM[2][1], HM[2][2])
        RZ = atan2(HM[1][0], HM[0][0])
        RY = atan2(-HM[2][0], HM[0][0] * cos(RZ) + HM[1][0] * sin(RZ))
        return [RX, RY, RZ]

#DH table definition
alpha1,alpha2,alpha3,alpha4,alpha5,alpha6=radians(90),radians(0),radians(0),radians(90),radians(-90),radians(0)
a1,a2,a3,a4,a5,a6=0,-425,-392.25,0,0,0
d1,d2,d3,d4,d5,d6=0,0,0,109.15,94.65,82.3

#Joints angle definition
j1,j2,j3,j4,j5,j6 = radians(23),radians(63),radians(33),radians(19),radians(10),radians(36.55)

#Homogenous Matrix defnition
tbase =TXZ(89.159,'z')
t1 = np.matmul(np.matmul(np.matmul(TXZ(d1,'z'),RXZ(j1,'z')),TXZ(a1,'x')),RXZ(alpha1,'x'))
t2 = np.matmul(np.matmul(np.matmul(TXZ(d2,'z'),RXZ(j2,'z')),TXZ(a2,'x')),RXZ(alpha2,'x'))
t3 = np.matmul(np.matmul(np.matmul(TXZ(d3,'z'),RXZ(j3,'z')),TXZ(a3,'x')),RXZ(alpha3,'x'))
t4 = np.matmul(np.matmul(np.matmul(TXZ(d4,'z'),RXZ(j4,'z')),TXZ(a4,'x')),RXZ(alpha4,'x'))
t5 = np.matmul(np.matmul(np.matmul(TXZ(d5,'z'),RXZ(j5,'z')),TXZ(a5,'x')),RXZ(alpha5,'x'))
t6 = np.matmul(np.matmul(np.matmul(TXZ(d6,'z'),RXZ(j6,'z')),TXZ(a6,'x')),RXZ(alpha6,'x'))

#Homogenous transfer matrix
ht=np.matmul(np.matmul(np.matmul(np.matmul(np.matmul(np.matmul(tbase,t1),t2),t3),t4),t5),t6)

#Print the Homogenous Matrix
print(ht)

#Print the location
Orientation = HM2RPY(ht)
print("X:", ht[0][3])
print("Y:", ht[1][3])
print("Z:", ht[2][3])

print("--------------------------")
#Print the orientation
print("RX:", np.rad2deg(Orientation[0]))
print("RY:", np.rad2deg(Orientation[1]))
print("RZ:", np.rad2deg(Orientation[2]))

The descriptive model above estimates the orientation and transformation of a UR5 colaborative robot. Meaning that for each joint space configuration, there is a corresponding cartesian orientation and location.

Random Robot Joints configuration and generation

// Example C++ program
#include <iostream>
#include <string>
#include <random>
float randFloat(float a, float b)
{
        //std::cout << (float)rand() << "\n";

        return ((b - a) * ((float)rand() / RAND_MAX)) + a;
}
int main()
{
        float j1, j2, j3, j4, j5, j6;
        int nbExec = 10;

        for (int i=0; i<nbExec; i++)
        {
                j1 = round(10 * randFloat(-180, 40)) / 10;
                j2 = round(10 * randFloat(-150,  0)) / 10;
                j3 = round(10 * randFloat(-150,  0)) / 10;
                j4 = round(10 * randFloat(-180,  0)) / 10;
                j5 = round(10 * randFloat(-180, 90)) / 10;
                j6 = round(10 * randFloat( -90, 90)) / 10;
                std::cout << "J1=" <<  j1 << " " << "J2=" <<  j2 << " "  <<"J3=" <<  j3 << " "  <<"J4=" <<  j4 << " "  <<"J5=" <<  j5 << " "  <<"J6=" <<  j6 << " "  <<"\n";
        }

}

The above code will generate uniformly random joints between a specific range.